#include <sys/time.h>
#include <iostream>
#include <vector>

#include "cxcore.h"
#include "cv.h"

#include "fitting.hh"
#include "other_functions.h"

//#define DEBUG_SHOW_INFO

typedef struct eval_pt_t{
    CvPoint pt;
    double val;
    unsigned int count;
} eval_pt_t;

typedef struct simplexData{
    boost::shared_ptr<IplImage> image;
    const IplImage *patch; //the patch to match to image
    DistanceType distT; 
    boost::shared_ptr<CvMat> match_pre; //precalculated matchings between patch and images
    CvRect match_pre_r; //the rectangle in image that corresponds to matching_pre

    std::vector<eval_pt_t> evpts;
}simplexData;



double eval_obj_function(const gsl_vector *v, void *params);

CvPoint simplex_xy(simplexData& sData, CvPoint seed,
		   double& pos_match);

double eval_distance(const IplImage *src1, const IplImage *src2, DistanceType distT)
{

    double dist;
    CvMat *aux = NULL;

    //check sizes
    CvSize s1 = cvGetSize(src1), s2 = cvGetSize(src2);
    if(s1.width != s2.width || s1.height != s2.height){
	std::cerr << "eval_distance: Bad sizes: sizes must be equal: (" 
		  << src1->width <<"x"<<src1->height << "), ("
		  << src2->width <<"x"<<src1->height << ")." << std::endl;	    
	exit(-1);
    }
    
//     aux = cvCreateMat(1,1, CV_32F);

 //    struct timeval t1,t2;
//     gettimeofday(&t1, NULL);


    if(distT == SSD){
	double d = cvNorm(src1, src2);
	d = d*d;
	dist = d / (cvNorm(src1)*cvNorm(src2));
    }
    else if(distT == NCC){
	IplImage *aux = cvCreateImage(s1, src1->depth, src1->nChannels);
	cvMul(src1, src2, aux);
	CvScalar d = cvSum(aux);
	dist = 1- (d.val[0] / (cvNorm(src1)*cvNorm(src2)));
	cvReleaseImage(&aux);
    }
    else if(distT == ZNCC){
	CvScalar m1, m2,d;
	m1 = cvAvg(src1);
	m2 = cvAvg(src2);
	IplImage *aux = cvCreateImage(s1, src1->depth, src1->nChannels);
	IplImage *aux1 = cvCreateImage(s1, src1->depth, src1->nChannels);
	IplImage *aux2 = cvCreateImage(s1, src1->depth, src1->nChannels);

	cvSubS(src1, m1, aux1);
	cvSubS(src2, m2, aux2);

	cvMul(aux1, aux2, aux);
	d = cvSum(aux);
	double norm = (cvNorm(aux1)*cvNorm(aux2));
	if (norm == 0)
	    dist = -4000;
	else 
	    dist = 1- (d.val[0] / norm);
	cvReleaseImage(&aux2);
	cvReleaseImage(&aux1);
	cvReleaseImage(&aux);

    }
    else{	//not yet
//     case MI:
//     default:
	dist = 10;
    }
    
    cvReleaseMat(&aux);
    
//     gettimeofday(&t2, NULL);
//     printf("time: %f\n", (float)(t2.tv_sec - t1.tv_sec) + (float)(t2.tv_usec - t1.tv_usec)/1.0e6);
    

    return dist;
}

double eval_projection_error(const IplImage *image, AppearanceBase& app_base, DistanceType distT)
{

    double error;
    if(!app_base.nvectors){
	std::cerr << "No appearance vectors found!" << std::endl;
	return 0;
    }

    //get the projection coefficients over the appearance base
    std::vector<double> coeff;
    boost::shared_ptr<IplImage> proj (cvCreateImage(cvGetSize(app_base.mean.get()),
						    app_base.mean->depth,
						    app_base.mean->nChannels),
				      deleteIplImage);
    
    //substract the mean
    cvSub(image, app_base.mean.get(), proj.get());

    for(uint ai=0; ai<app_base.nvectors; ai++){
	double val = cvDotProduct(proj.get(), app_base.vectors.at(ai).get());
	coeff.push_back(val);

    }
    
    //impose some limit on the parameters
    uint ovlim=0;
    for(uint ai=0; ai<app_base.nvectors; ai++){
	double lim = //  sqrt(3.0)*
	    cvGetReal1D(app_base.eigvals.get(), ai);
	if(fabs(coeff.at(ai)) > lim){	    
	    if(coeff.at(ai)<0)
		coeff.at(ai) = -1*lim;
	    else
		coeff.at(ai) = lim;

	    ovlim++;
	}
    }

    //and project back
    cvCopy(app_base.mean.get(), proj.get());
    for(uint ai=0;ai<app_base.nvectors;ai++){
	cvAddWeighted(proj.get(), 1, app_base.vectors.at(ai).get(), 
		      coeff.at(ai),0, proj.get());
    }
    
    switch(distT){
    case SSD:
    case NCC:
    case ZNCC:
	error = eval_distance(image, proj.get(), distT);
	break;
   case MI:
       //not yet
       break;
   }


    return error;
}


/****************/

// static CvPoint refine_seed_xy(const IplImage *image, const IplImage *patch, 
// 			      std::vector<CvPoint> probe_pts, DistanceType distT,
// 			      std::vector<double>& matches, 
// 			      std::vector<double>::iterator& best_match)
// {
    
//     unsigned int best_idx = 0;        
//     double best = 10e10;

//     for(uint ai=0; ai<probe_pts.size(); ai++){
	
// 	CvMat submat;
// 	CvPoint p = probe_pts.at(ai);
// 	cvGetSubRect(image, &submat, cvRect(p.x, p.y, patch->width, patch->height));	
// 	IplImage subimg;
// 	cvGetImage(&submat, &subimg);

// 	double val = eval_distance(&subimg, patch, distT);
// 	if (val < best){
// 	    best_idx = ai;
// 	    best = val;
// 	}
	
// 	matches.push_back(val);
//     }

//     best_match = matches.begin()+best_idx;

//     return probe_pts.at(best_idx);
// }



CvPoint fit_patch_xy(const IplImage *src_image, const IplImage *patch, CvPoint seed0,
		     std::vector<double>& matches, DistanceType distT, 
		     unsigned int grid_size, unsigned int grid_step)
{

    CvPoint fitted;
    double best = 10e10;
//     std::vector<double>::iterator iter;
    CvPoint seed = seed0;
    simplexData sData;
    boost::shared_ptr<IplImage> image (cvCloneImage(src_image), deleteIplImage);

    sData.patch = patch;
    sData.image = image;   
    sData.distT = distT;

    //sample on some points around the seed.
    if(grid_size > 1){	
	
	CvRect imr = cvRect(0,0, image->width, image->height);
	CvRect mat_r;

	mat_r = cvRect((-1*((int)grid_size)/2)*grid_step + seed.x,
		       (-1*((int)grid_size)/2)*grid_step + seed.y,
		       grid_size*grid_step + patch->width, 
		       grid_size*grid_step + patch->height);
	
	//check it is not outside the image
	CvRect r = cvMaxRect(&mat_r,&imr);
	
	if(r.x != imr.x)
	    mat_r.x = imr.x;
	if(r.y !=imr.y)
	    mat_r.y = imr.y;
	if(r.width != imr.width)
	    mat_r.width = imr.width - mat_r.x;
	if(r.height != imr.height)
	    mat_r.height = imr.height - mat_r.y;

	CvSize s = cvGetSize(image.get());
        //printf("%dx%d, (%d,%d, %dx%d) (%dx%d)\n",s.width, s.height , 	   mat_r.x, mat_r.y, mat_r.width, mat_r.height, patch->width, patch->height);fflush(stdout);
	if (mat_r.width <1 || mat_r.height<1)
	{
	    fitted.x = -1;
	    return fitted;
	}
	cvSetImageROI(image.get(), mat_r);

    
	sData.match_pre_r = cvRect(mat_r.x, mat_r.y, 
				   mat_r.width+1 -patch->width,
				   mat_r.height+1 - patch->height);
        //printf("%dx%d, (%d,%d, %dx%d) (%dx%d)\n", sData.match_pre_r.width, sData.match_pre_r.height, 	   mat_r.x, mat_r.y, mat_r.width, mat_r.height, patch->width, patch->height);fflush(stdout);
	if (sData.match_pre_r.width < 1 || sData.match_pre_r.height < 1)
	{
	    fitted.x = -1;
	    return fitted;
	}
	sData.match_pre.reset(cvCreateMat(sData.match_pre_r.height,
					  sData.match_pre_r.width, CV_32F), deleteCvMat);


	switch(distT){
	case SSD:
	    cvMatchTemplate( image.get(),  patch, sData.match_pre.get(), distT);
	    break;
	case NCC:
	case ZNCC:
	    cvMatchTemplate( image.get(),  patch, sData.match_pre.get(), distT);
	    cvSubRS(sData.match_pre.get(), cvScalar(1), sData.match_pre.get());
	    break;
	case MI:
	    //not yet
	    break;
	}

	cvResetImageROI(image.get());
	double w;
	
	cvMinMaxLoc( sData.match_pre.get(), &best, &w, &seed);
	seed.x += sData.match_pre_r.x;
	seed.y += sData.match_pre_r.y;

	matches.push_back(w);
    }
    
    //optimize using the best seed.
    double positive, limit;


    fitted = simplex_xy(sData, seed, positive);
    
    if(positive > best){
	fitted = seed;
	positive = best;
    }

    limit = (positive+matches[0])/2;
    for(uint ai=0; ai< grid_size; ai++){
	for(uint bi=0; bi< grid_size; bi++){
	    double v;
	    int x,y;
	    x = ai*grid_step;
	    y = bi*grid_step;
	    
	    CvSize s = cvGetSize(sData.match_pre.get());
	    if (s.width <= x || s.height <= y)
	    {
		continue;
	    }
	    if((abs(x- (fitted.x -sData.match_pre_r.x)) + abs(y - (fitted.y - sData.match_pre_r.y)))< 1.2*grid_step){

		//printf("=== y:%d x:%d -- w:%d, h:%d", y, x, s.width, s.height);fflush(stdout);
		v = cvmGet(sData.match_pre.get(), y,x);
		if(v > limit)
		    matches.push_back(v);
		//printf("***\n");fflush(stdout);

	    }
	}		
    }

    matches.push_back(positive);

#ifdef DEBUG_SHOW_INFO
    for(uint ai=0; ai<sData.evpts.size(); ai++){
	std::cout << "pt: " << sData.evpts[ai].pt.x << ", " << sData.evpts[ai].pt.y
		  <<", count= " << sData.evpts[ai].count << std::endl;
    }
#endif

    return fitted;

}


double eval_obj_function(const gsl_vector *v, void *params)
{

    simplexData *sData = (simplexData*)params;
    double val;

    CvRect r = cvRect((int)gsl_vector_get(v, 0),
		      (int)gsl_vector_get(v, 1),
		      sData->patch->width, sData->patch->height);
    CvRect br;

//     printf("eval_obj_funct: %d, %d\n", r.x, r.y);
    br.x = r.x - sData->match_pre_r.x;
    br.y = r.y - sData->match_pre_r.y;
    if(br.x>= 0 && br.x < sData->match_pre_r.width &&
       br.y >= 0 && br.y < sData->match_pre_r.height){

	val = cvmGet(sData->match_pre.get(), br.y, br.x);
	return val;
    }
    else{
	for(uint ai=0; ai < sData->evpts.size(); ai++){
	    if(r.x == sData->evpts[ai].pt.x && r.y == sData->evpts[ai].pt.y){
		sData->evpts[ai].count++;
		return sData->evpts[ai].val;
	    }		
	}
    }


    CvRect imr = cvRect(0,0,sData->image->width, sData->image->height);
    br = cvMaxRect(&r, &imr);

    if(br.x != imr.x || br.y !=imr.y || br.width != imr.width ||
       br.height != imr.height){
	return 10;

    }


    cvSetImageROI(sData->image.get(),r);
    CvSize sz = cvGetSize(sData->image.get());

    val = eval_distance(sData->image.get(), sData->patch, sData->distT);
    if (val > 4000) val = 4000;
    else if (val < -4000) val = -4000;
    cvResetImageROI(sData->image.get());

    eval_pt_t ep;
    ep.pt = cvPoint(r.x, r.y);
    ep.val = val;
    ep.count = 1;

    sData->evpts.push_back(ep);

    return val;

}


CvPoint simplex_xy(simplexData& sData, CvPoint seed, double& positive_match)
{
    
    const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex;
    gsl_multimin_fminimizer *s = NULL;
    gsl_vector *ss, *x;
    gsl_multimin_function minex_func;


    unsigned int np = 2;
    size_t iter = 0;
    int status;
    double size =0;

    CvPoint positive;
//     CvSize p_sz = cvGetSize(patch);
//     CvRect imr = cvRect(0,0, image->width, image->height);

    /* Initial vertex size vector */
    ss = gsl_vector_alloc (np);
    
    gsl_vector_set_all(ss, NM_COEFF_STEP_SIZE);

    /* Starting point */
    x = gsl_vector_alloc (np);
    gsl_vector_set(x, 0, seed.x);
    gsl_vector_set(x, 1, seed.y);  

    /* Initialize method and iterate */
    minex_func.f = &eval_obj_function;
    minex_func.n = np;
    minex_func.params = &sData;
    
    s = gsl_multimin_fminimizer_alloc (T, np);
    gsl_multimin_fminimizer_set (s, &minex_func, x, ss);
    

    do
	{
	    status = gsl_multimin_fminimizer_iterate(s);
	    
	    if (status)
		break;
	    
	    size = gsl_multimin_fminimizer_size (s);
	    status = gsl_multimin_test_size (size, 1);
	    
	  //   else if(1){
// 		printf ("%5d, f() = %.7f size = %.5f\n", iter, s->fval, size); 
// 	    }
	    
	    //recover the parameters

	    positive.x = (int)gsl_vector_get(s->x, 0);
	    positive.y = (int)gsl_vector_get(s->x, 1);
		 
	    positive_match = s->fval;

	   //  if(iter == 0)
// 		printf ("%5d, f() = %.7f\n", iter, s->fval); 

// 	    if (status == GSL_SUCCESS){
// 		printf ("converged to minimum at: ");
// 		printf ("%5d ", iter);
// 		printf ("f() = %.7f, size = %.5f\n", s->fval, size);
// 	    }

	   //  //check the patch is not outside the image
// 	    else{
// 		CvRect r = cvRect((int)gsl_vector_get(s->x, 0),
// 				   (int)gsl_vector_get(s->x, 1),
// 				   p_sz.width,
// 				   p_sz.height);
		    
// 		CvRect br = cvMaxRect(&r,&imr);
// 		if(br.x != imr.x || br.y !=imr.y || br.width != imr.width ||
// 		   br.height != imr.height){
// 		    std::cout << "Patch outside image: returning given seed" 
// 			      << std::endl;
		    
// 		    positive = seed;
// 		    positive_match = 1e10;
// 		    break;
// 		}


// 	    }


	    iter++;
	}while (status == GSL_CONTINUE && iter < 20);   

#ifdef DEBUG_SHOW_INFO
    if(iter >= 20){
	printf ("Did not converged. minimum at: ");
	printf ("%5d ", iter);
	printf ("f() = %.7f, size = %.5f\n", s->fval, size);
    }
#endif	

    gsl_vector_free(x);
    gsl_vector_free(ss);
    gsl_multimin_fminimizer_free (s);


    return positive;

}
